#include "Polyhedron.h"

#define PI 3.14159265
#define POS_INF	1e10

Polyhedron::Polyhedron(void)
{
}


Polyhedron::~Polyhedron(void)
{
}

void Polyhedron::buildPolyStructure( initializeFunc setUp )
{
	setUp(&edgeList, &vertList, &faceList, &faceCount, &vertCount);
	printf("hello\n");
	setUpLines();

	constructFaceNormal();

	constructVertNormal();
}

Vert Polyhedron::getVertexFromVertex( int index )
{
	return vertList[index];
}

Tri Polyhedron::getFaceFromFace( int index )
{
	Tri t;
	Edge e = edgeList[faceList[index].edge];

	t.a = e.vert;

	e = edgeList[e.next];

	t.b = e.vert;

	e = edgeList[e.next];

	t.c = e.vert;

	return t;
}

void Polyhedron::getOneRingVertexFromVertex( int index, IndexList** ring )
{
	int start, i;

	Vert v = vertList[index];
	Edge e = edgeList[v.edge];

	start = getVertexFromEdge(v.edge);

	(*ring)->push_back(start);

	i = getVertexFromEdge(e.next);

	e = edgeList[e.next];

	(*ring)->push_back(i);

	while (i != start)
	{
		e = edgeList[e.next];
		e = edgeList[e.pair];		

		i = getVertexFromEdge(e.next);

		e = edgeList[e.next];

		if (i != start)
		{
			(*ring)->push_back(i);
		}		
	}

}

int Polyhedron::getVertexFromEdge( int index )
{	
	Edge e = edgeList[index];
	Edge ne = edgeList[e.next];

	return ne.vert;
}

void Polyhedron::getOneRingFaceFromVertex( int index, IndexList** ring )
{
	int start, i;

	Vert v = vertList[index];
	Edge e = edgeList[v.edge];

	start = e.face;

	(*ring)->push_back(start);

	e = edgeList[e.next];
	e = edgeList[e.next];
	e = edgeList[e.pair];

	i = e.face;

	(*ring)->push_back(i);

	while (i != start)
	{
		e = edgeList[e.next];
		e = edgeList[e.next];
		e = edgeList[e.pair];

		i = e.face;

		if (i != start)
		{
			(*ring)->push_back(i);
		}
	}
}

void Polyhedron::getIncidentFaceFromEdge( int index, int** ops )
{
	Edge e = edgeList[index];

	(*ops)[0] = e.face;

	e = edgeList[e.pair];

	(*ops)[1] = e.face;
}

Tri Polyhedron::getEdgeFromFace( int index )
{
	Face f = faceList[index];
	Edge e = edgeList[f.edge];

	Tri tri;

	tri.a = f.edge;
	tri.b = e.next;

	e = edgeList[e.next];

	tri.c = e.next;

	return tri;
}

Line Polyhedron::getLineFromEdge( int index )
{
	Edge e = edgeList[index];
	Line l;

	l.a = e.vert;
	l.b = getVertexFromEdge(index);

	return l;
}

void Polyhedron::constructFaceNormal()
{
	for (int i=0; i<faceCount; i++)
	{
		Tri te = getEdgeFromFace(i);		

		Line la = getLineFromEdge(te.a);
		Line lb = getLineFromEdge(te.b);

		Vert vs = vertList[la.a];
		Vert ve = vertList[la.b];

		Vec3d da(ve.x - vs.x, ve.y - vs.y, ve.z - vs.z);

		vs = vertList[lb.a];
		ve = vertList[lb.b];

		Vec3d db(ve.x - vs.x, ve.y - vs.y, ve.z - vs.z);

		Vec3d fp(vs.x, vs.y, vs.z);

		Vec3d fn = normalized(cross(da, db));

		faceList[i].nx = fn[0];
		faceList[i].ny = fn[1];
		faceList[i].nz = fn[2];
		faceList[i].d = dot(fp, fn);
	}
}

void Polyhedron::constructVertNormal()
{
	for (int i=0; i<vertCount; i++)
	{
		IndexList* ringF = new IndexList();

		vertList[i].nx = vertList[i].ny = vertList[i].nz = 0;

		getOneRingFaceFromVertex(i, &ringF);

		for (int j=0; j<ringF->size(); j++)
		{
			double angle = getIncidentFaceAngle(ringF->at(j), i);

			Face af = faceList[ringF->at(j)];

			vertList[i].nx += af.nx * angle;
			vertList[i].ny += af.ny * angle;
			vertList[i].nz += af.nz * angle;
		}

		Vec3d tmpN(vertList[i].nx, vertList[i].ny, vertList[i].nz);

		tmpN = normalized(tmpN);

		vertList[i].nx = tmpN[0];
		vertList[i].ny = tmpN[1];
		vertList[i].nz = tmpN[2];

		ringF->clear();

		delete ringF;
	}
}

double Polyhedron::getIncidentFaceAngle( int f_Index, int v_Index )
{
	double angle;	

	Tri ft = getFaceFromFace(f_Index);

	if (ft.b == v_Index)
	{
		ft.b = ft.a;
		ft.a = v_Index;
	}else if (ft.c == v_Index)
	{
		ft.c = ft.a;
		ft.a = v_Index;
	}

	Vert vs = vertList[ft.a];
	Vert ve = vertList[ft.b];

	Vec3d da(ve.x - vs.x, ve.y - vs.y, ve.z - vs.z);

	vs = vertList[ft.a];
	ve = vertList[ft.c];

	Vec3d db(ve.x - vs.x, ve.y - vs.y, ve.z - vs.z);

	da = normalized(da);
	db = normalized(db);

	angle = dot(da, db);

	return acos(angle);
}

Vec3d Polyhedron::getPointFromST( int f_Index, double s, double t )
{
	Tri tf = getFaceFromFace(f_Index);

	Vert v0 = vertList[tf.a];
	Vert v1 = vertList[tf.b];
	Vert v2 = vertList[tf.c];

	return (1 - s - t) * Vec3d(v0.x, v0.y, v0.z) + s * Vec3d(v1.x, v1.y, v1.z) + t * Vec3d(v2.x, v2.y, v2.z);
}

Vec3d Polyhedron::getNormalFromST( int f_Index, double s, double t )
{
	Tri tf = getFaceFromFace(f_Index);

	Vert v0 = vertList[tf.a];
	Vert v1 = vertList[tf.b];
	Vert v2 = vertList[tf.c];

	return (1 - s - t) * Vec3d(v0.nx, v0.ny, v0.nz) + s * Vec3d(v1.nx, v1.ny, v1.nz) + t * Vec3d(v2.nx, v2.ny, v2.nz);
}

void Polyhedron::getSqrDistance( Vec3d pt, double* sqrDist, Vec3d* closestPt, Vec3d* closestNormal )
{
	double bestDist = POS_INF;
	double bestS = 0;
	double bestT = 0;
	int bestFace = -1;

	for (int i=0; i<faceCount; i++)
	{
		double curDist;
		double curS, curT;

		getSqrDistanceToFace(i, pt, &curDist, &curS, &curT);
		
		if (curDist < bestDist)
		{
			bestDist = curDist;
			bestS = curS;
			bestT = curT;
			bestFace = i;
		}
	}

	(*sqrDist) = bestDist;
	(*closestPt) = getPointFromST(bestFace, bestS, bestT);
	(*closestNormal) = getNormalFromST(bestFace, bestS, bestT);

}

void Polyhedron::translatePolyhedron( Vec3d translate )
{
	for (int i=0; i<vertCount; i++)
	{
		vertList[i].x = vertList[i].x + translate[0];
		vertList[i].y = vertList[i].y + translate[1];
		vertList[i].z = vertList[i].z + translate[2];
	}
}

void Polyhedron::rotatePolyhedron( Mat33d rotate )
{
	for (int i=0; i<vertCount; i++)
	{
		Vec3d vert(vertList[i].x, vertList[i].y, vertList[i].z);

		vert = rotate * vert;		

		vertList[i].x = vert[0];
		vertList[i].y = vert[1];
		vertList[i].z = vert[2];

	}

	constructFaceNormal();

	constructVertNormal();

}

void Polyhedron::setUpLines()
{
	lineCount = faceCount * 3 / 2;

	int* check = (int*)malloc(lineCount * 2 * sizeof(int));
	lineList = (Line*)malloc(lineCount * sizeof(Line));

	for (int i=0; i<lineCount*2; i++)
	{
		check[i] = 1;
	}

	int index = 0;

	for (int i=0; i<lineCount*2; i++)
	{
		if (check[i])
		{
			Edge e = edgeList[i];

			lineList[index].a = e.vert;
			lineList[index].b = getVertexFromEdge(i);

			check[i] = 0;
			check[e.pair] = 0;

			index++;
		}
	}

	//printf("%d\n", lineCount);

	delete check;
}

void Polyhedron::getSqrDistanceToFace( int f_Index, Vec3d pt, double* fSqrDist, double* fS, double* fT )
{
	Tri tf = getFaceFromFace(f_Index);

	Vert v0 = vertList[tf.a];
	Vert v1 = vertList[tf.b];
	Vert v2 = vertList[tf.c];

	Vec3d diff = Vec3d(v0.x, v0.y, v0.z) - pt;
	Vec3d edge0 = Vec3d(v1.x, v1.y, v1.z) - Vec3d(v0.x, v0.y, v0.z);
	Vec3d edge1 = Vec3d(v2.x, v2.y, v2.z) - Vec3d(v0.x, v0.y, v0.z);


	double a00 = mag2(edge0);// edge0.SquaredLength();
	double a01 = dot(edge0, edge1);// edge0.Dot(edge1);
	double a11 = mag2(edge1);//edge1.SquaredLength();
	double b0 = dot(diff, edge0);//diff.Dot(edge0);
	double b1 = dot(diff, edge1);//diff.Dot(edge1);
	double c = mag2(diff);//diff.SquaredLength();
	double det = abs(a00 * a11 - a01 * a01);//Math<Real>::FAbs(a00*a11 - a01*a01);
	double s = a01 * b1 - a11 * b0;
	double t = a01 * b0 - a00 * b1;
	double sqrDistance;

	if (s + t <= det)
	{
		if (s < (double)0)
		{
			if (t < (double)0)  // region 4
			{
				if (b0 < (double)0)
				{
					t = (double)0;
					if (-b0 >= a00)
					{
						s = (double)1;
						sqrDistance = a00 + ((double)2)*b0 + c;
					}
					else
					{
						s = -b0/a00;
						sqrDistance = b0*s + c;
					}
				}
				else
				{
					s = (double)0;
					if (b1 >= (double)0)
					{
						t = (double)0;
						sqrDistance = c;
					}
					else if (-b1 >= a11)
					{
						t = (double)1;
						sqrDistance = a11 + ((double)2)*b1 + c;
					}
					else
					{
						t = -b1/a11;
						sqrDistance = b1*t + c;
					}
				}
			}
			else  // region 3
			{
				s = (double)0;
				if (b1 >= (double)0)
				{
					t = (double)0;
					sqrDistance = c;
				}
				else if (-b1 >= a11)
				{
					t = (double)1;
					sqrDistance = a11 + ((double)2)*b1 + c;
				}
				else
				{
					t = -b1/a11;
					sqrDistance = b1*t + c;
				}
			}
		}
		else if (t < (double)0)  // region 5
		{
			t = (double)0;
			if (b0 >= (double)0)
			{
				s = (double)0;
				sqrDistance = c;
			}
			else if (-b0 >= a00)
			{
				s = (double)1;
				sqrDistance = a00 + ((double)2)*b0 + c;
			}
			else
			{
				s = -b0/a00;
				sqrDistance = b0*s + c;
			}
		}
		else  // region 0
		{
			// minimum at interior point
			double invDet = ((double)1)/det;
			s *= invDet;
			t *= invDet;
			sqrDistance = s*(a00*s + a01*t + ((double)2)*b0) +
				t*(a01*s + a11*t + ((double)2)*b1) + c;
		}
	}
	else
	{
		double tmp0, tmp1, numer, denom;

		if (s < (double)0)  // region 2
		{
			tmp0 = a01 + b0;
			tmp1 = a11 + b1;
			if (tmp1 > tmp0)
			{
				numer = tmp1 - tmp0;
				denom = a00 - ((double)2)*a01 + a11;
				if (numer >= denom)
				{
					s = (double)1;
					t = (double)0;
					sqrDistance = a00 + ((double)2)*b0 + c;
				}
				else
				{
					s = numer/denom;
					t = (double)1 - s;
					sqrDistance = s*(a00*s + a01*t + ((double)2)*b0) +
						t*(a01*s + a11*t + ((double)2)*b1) + c;
				}
			}
			else
			{
				s = (double)0;
				if (tmp1 <= (double)0)
				{
					t = (double)1;
					sqrDistance = a11 + ((double)2)*b1 + c;
				}
				else if (b1 >= (double)0)
				{
					t = (double)0;
					sqrDistance = c;
				}
				else
				{
					t = -b1/a11;
					sqrDistance = b1*t + c;
				}
			}
		}
		else if (t < (double)0)  // region 6
		{
			tmp0 = a01 + b1;
			tmp1 = a00 + b0;
			if (tmp1 > tmp0)
			{
				numer = tmp1 - tmp0;
				denom = a00 - ((double)2)*a01 + a11;
				if (numer >= denom)
				{
					t = (double)1;
					s = (double)0;
					sqrDistance = a11 + ((double)2)*b1 + c;
				}
				else
				{
					t = numer/denom;
					s = (double)1 - t;
					sqrDistance = s*(a00*s + a01*t + ((double)2)*b0) +
						t*(a01*s + a11*t + ((double)2)*b1) + c;
				}
			}
			else
			{
				t = (double)0;
				if (tmp1 <= (double)0)
				{
					s = (double)1;
					sqrDistance = a00 + ((double)2)*b0 + c;
				}
				else if (b0 >= (double)0)
				{
					s = (double)0;
					sqrDistance = c;
				}
				else
				{
					s = -b0/a00;
					sqrDistance = b0*s + c;
				}
			}
		}
		else  // region 1
		{
			numer = a11 + b1 - a01 - b0;
			if (numer <= (double)0)
			{
				s = (double)0;
				t = (double)1;
				sqrDistance = a11 + ((double)2)*b1 + c;
			}
			else
			{
				denom = a00 - ((double)2)*a01 + a11;
				if (numer >= denom)
				{
					s = (double)1;
					t = (double)0;
					sqrDistance = a00 + ((double)2)*b0 + c;
				}
				else
				{
					s = numer/denom;
					t = (double)1 - s;
					sqrDistance = s*(a00*s + a01*t + ((double)2)*b0) +
						t*(a01*s + a11*t + ((double)2)*b1) + c;
				}
			}
		}
	}

	if (sqrDistance < 0.0)
	{
		sqrDistance = 0.0;
	}

	*fSqrDist = sqrDistance;
	*fS = s;
	*fT = t;
}


